using System;
using Microsoft.SPOT;

namespace Robot
{
    class RobotController
    {
        PWM m_engineLeft;
        PWM m_engineRight;

        /// <summary>
        /// Constructor.
        /// </summary>
        /// <param name="leftWheel">Output pin for left engine's PWM signal</param>
        /// <param name="rightWheel">Output pin for right engine's PWM signal</param>
        public RobotController(Microsoft.SPOT.Hardware.Cpu.Pin leftWheel, Microsoft.SPOT.Hardware.Cpu.Pin rightWheel)
        {
            m_engineLeft = new PWM(leftWheel, 400);
            m_engineRight = new PWM(rightWheel, 400);
        }

        private Int16 m_speed;
        private Int16 m_direction;

        /// <summary>
        /// Sets and gets speed, spanning from -50 to 50
        /// </summary>
        public Int16 Speed
        {
            get
            {
                return m_speed;
            }
            set
            {
                if (value > 50)
                {
                    m_speed = 50;
                }
                else if (value < -50)
                {
                    m_speed = -50;
                }
                else
                {
                    m_speed = value;
                }
                UpdatePWM();
            }
        }

        /// <summary>
        /// Sets and gets direction, spanning from -50 to 50
        /// </summary>
        public Int16 Direction
        {
            get
            {
                return m_direction;
            }
            set
            {
                if (value > 50)
                {
                    m_direction = 50;
                }
                else if (value < -50)
                {
                    m_direction = -50;
                }
                else
                {
                    m_direction = value;
                }
                UpdatePWM();
            }
        }

        /// <summary>
        /// Gets duty cycle of left engine
        /// </summary>
        public byte PWM_Left
        {
            get
            {
                return m_engineLeft.DutyCycle;
            }
        }

        /// <summary>
        /// Gets duty cycle of right engine
        /// </summary>
        public byte PWM_Right
        {
            get
            {
                return m_engineRight.DutyCycle;
            }
        }

        /// <summary>
        /// Calculates and sets PWM values in the two PWMs
        /// </summary>
        private void UpdatePWM()
        {
            // This makes it more logical to turn while reversing
            short dir = (short)(Speed >= 0 ? Direction : -Direction);

            // Calculate left speed based on speed and direction
            int val = Speed + 50 + dir;
            // Convert speed into duty cycle (use 1 to 99, 0 for stop)
            m_engineLeft.DutyCycle = (byte)(val == 50 ? 0 : (val > 99 ? 99 : (val < 1 ? 1 : val)));
            // For debugging...
            System.Diagnostics.Debug.Print("Left: " + m_engineLeft.DutyCycle.ToString());

            // Calculate right speed based on speed and direction
            val = Speed + 50 - dir;
            // Convert speed into duty cycle (subtract from 100, since engines are "mirrored"
            m_engineRight.DutyCycle = (byte)(val == 50 ? 0 : (100 - (val > 99 ? 99 : (val < 1 ? 1 : val))));
            // For debugging...
            System.Diagnostics.Debug.Print("Right: " + m_engineRight.DutyCycle.ToString());
        }
    }
}
